#!/usr/bin/python
# coding=utf-8
import time
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int8


class adjust_clean:
    def __init__(self):
        self.cmd_pub = rospy.Publisher(
            "/cmd_vel", Twist, queue_size=10)			# 发布运动控制信息
        self.moveclean_cmd = rospy.Publisher(
            "/moveclean_cmd", Int8, queue_size=10)
        self.clean_cmd = rospy.Publisher("/clean_cmd", Int8, queue_size=10)
        self.moveclean_report_sub = rospy.Subscriber(
            "/moveclean_report", Int8, self.moveCleanReportCallback)
        self.clean_report_sub = rospy.Subscriber(
            "/clean_report", Int8, self.cleanReportCallback)
        self.start_adjust_flag = True
        self.start_clean_flag = True
        self.finish_clean_flag = True
        if self.start_adjust_flag:
            moveclean_adjust_flag = Int8()
            moveclean_adjust_flag.data = 1
            self.moveclean_cmd.publish(moveclean_adjust_flag)
            rospy.loginfo("start adjust clean...")
    
    # 0 normal,1 get start ironing,2 get start cleaning, 3 finish cleaning,
    # 4 get force stop cleaning, 5 request stop moving, 6 request moving
    def cleanReportCallback(self, msg):
        report_clean_flag = msg.data
        if report_clean_flag == 2:  # 清洁板收到开始清洁命令
            self.start_clean_flag = False
        elif report_clean_flag == 5:  # 清洁板请求停止运动
            twist = Twist()
            twist.linear.x = 0
            twist.angular.z = 0
            try:
                self.cmd_pub.publish(twist)
            except CvBridgeError as e:
                print e
        elif report_clean_flag == 6:  # 清洁板请求运动
            twist = Twist()
            twist.linear.x = 0.02
            twist.angular.z = 0
            try:
                self.cmd_pub.publish(twist)
            except CvBridgeError as e:
                print e
        elif report_clean_flag == 3:  # 清洁板上报清洁结束
            if self.finish_clean_flag:
                i = 0
                while(1):
                    time.sleep(1)  # 刘鹤要求
                    clean_flag = Int8()
                    moveclean_flag = Int8()
                    clean_flag.data = 5  # 3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                    moveclean_flag.data = 3  # 2 start clean,3 stop clean
                    self.clean_cmd.publish(clean_flag)
                    self.moveclean_cmd.publish(moveclean_flag)
                    print "finish clean"
                    # self.finish_clean_flag = False
                    i += 1
                    if(i == 1):
                        break

    def moveCleanReportCallback(self, msg):
        moveclean_report_status = msg.data
        if moveclean_report_status == 1:  # 控制板收到开始对位
            self.start_adjust_flag = False
        elif moveclean_report_status == 2:  # 控制板发送对位结束，开始清洁
            if self.start_clean_flag:
                i = 0
                while(1):
                    time.sleep(3)  # 刘鹤要求
                    moveclean_flag = Int8()
                    clean_flag = Int8()
                    moveclean_flag.data = 2  # start clean 0x27
                    clean_flag.data = 3
                    # 2 start clean,3 stop clean
                    self.moveclean_cmd.publish(moveclean_flag)
                    # 3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                    self.clean_cmd.publish(clean_flag)
                    print "finish adjust and start clean"
                    i += 1
                    if(i == 1):
                        break
        elif moveclean_report_status == 3:  # 控制板收到开始清洁
            self.start_clean_flag = False
        elif moveclean_report_status == 4:  # 控制板收到开始清洁结束
            self.finish_clean_flag = False


if __name__ == '__main__':
    try:
        rospy.init_node("adjust_node")
        rospy.loginfo("Starting adjust node")
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down adjust node."
        cv.destroyAllWindows()
